#include "stepping_motor.h"

 int current_x = 150;
 int current_y = 150;


void delay_ms_for_stepping_motor(uint32_t ms)
{
    mspm0_delay_ms(ms);
}


void motor_step(void)
{
    // 设置 GPIO 引脚状态
    if (stepSequence[sequenceIndex][0] == 1)
        DL_GPIO_setPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN1_PIN);
    else
        DL_GPIO_clearPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN1_PIN);

    if (stepSequence[sequenceIndex][1] == 1)
        DL_GPIO_setPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN2_PIN);
    else
        DL_GPIO_clearPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN2_PIN);

    if (stepSequence[sequenceIndex][2] == 1)
        DL_GPIO_setPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN3_PIN);
    else
        DL_GPIO_clearPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN3_PIN);

    if (stepSequence[sequenceIndex][3] == 1)
        DL_GPIO_setPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN4_PIN);
    else
        DL_GPIO_clearPins(GPIO_STEPPING_MOTOR_PORT, GPIO_STEPPING_MOTOR_PIN_STEPPING_MOTOR_IN4_PIN);

    // 更新索引
    sequenceIndex += direction;
    int totalSteps = sizeof(stepSequence) / sizeof(stepSequence[0]);

    if (sequenceIndex >= totalSteps)
    {
        sequenceIndex = 0;
    }
    else if (sequenceIndex < 0)
    {
        sequenceIndex = totalSteps - 1;
    }
}



void motor_rotate(float angle, int dir)
{
    // 每圈步数（半步模式）：512
    float stepsPerDegree = 512.0 / 360.0;
    int steps = (int)(angle * stepsPerDegree + 0.5);  // 四舍五入

    direction = dir; 

    for (int i = 0; i < steps * 8; i++) {
        motor_step();       // X轴电机
        delay_ms_for_stepping_motor(2);
        }
}


// void move_to(float x_mm, float y_mm) {
//     int target_x = (int)(x_mm / STEP_TO_MM_X);
//     int target_y = (int)(y_mm / STEP_TO_MM_Y);

//     int dx = target_x - current_x; // PD算法
//     int dy = target_y - current_y;

//     // 控制X轴（motor_step_down）
//     if (dx > 0) {
//         for (int i = 0; i < dx; i++) {
//             direction_down = 1;      // X轴正转
//             motor_step_down();       // X轴电机
//             delay_ms_for_stepping_motor(5);
//         }
//     } else if (dx < 0) {
//         for (int i = 0; i < -dx; i++) {
//             direction_down = -1;     // X轴反转
//             motor_step_down();       // X轴电机
//             delay_ms_for_stepping_motor(5);
//         }
//     }

//     // 控制Y轴（motor_step_up）
//     if (dy > 0) {
//         for (int i = 0; i < dy; i++) {
//             direction = 1;           // Y轴正转
//             motor_step_up();         // Y轴电机
//             delay_ms_for_stepping_motor(5);
//         }
//     } else if (dy < 0) {
//         for (int i = 0; i < -dy; i++) {
//             direction = -1;          // Y轴反转
//             motor_step_up();         // Y轴电机
//             delay_ms_for_stepping_motor(5);
//         }
//     }

//     // 更新当前位置
//     current_x = target_x;
//     current_y = target_y;
// }

// void draw_square(float size_mm) 
// {
//     // 从原点开始画正方形
//     move_to(0, 0);                 // 起点 (0,0)
//     move_to(size_mm, 0);           // 向右移动 X轴（motor_down）
//     move_to(size_mm, size_mm);     // 向上移动 Y轴（motor_up）
//     move_to(0, size_mm);           // 向左移动 X轴（motor_down）
//     move_to(0, 0);                 // 向下移动 Y轴（motor_up）
// }

// void draw_circle(float radius_mm) 
// {
//     int steps = 360;  // 分360个点
//     DL_GPIO_setPins(GPIO_laser_PORT,GPIO_laser_PIN_laser_PIN);
//     for (int i = 0; i < steps; i++) {
//         float angle = 2 * 3.14159 * i / steps;
//         float x = radius_mm * cos(angle);
//         float y = radius_mm * sin(angle);
//         move_to(x, y);
//     }
//     DL_GPIO_clearPins(GPIO_laser_PORT,GPIO_laser_PIN_laser_PIN);
// }

// void draw_sine_wave(float start_x_mm, float end_x_mm) {
//     // 计算总点数
//     int total_points = (int)((end_x_mm - start_x_mm) / (WAVE_LENGTH_MM / WAVE_POINTS));
    
//     // 转换为弧度
//     float x_start = 0.0f;
//     float x_end = (end_x_mm - start_x_mm) * (2 * M_PI) / WAVE_LENGTH_MM;
//     float dx = (x_end - x_start) / total_points;

//     // 移动到起点
//     //move_to(start_x_mm, 0.0f);

//     // 绘制正弦波
//     for (int i = 0; i <= total_points; i++) {
//         float x = x_start + i * dx;
//         float y = WAVE_AMPLITUDE_MM * sin(x);  // y = A * sin(x)
//         move_to(start_x_mm + (x / (2 * M_PI)) * WAVE_LENGTH_MM, y);
//     }

//     for (int i = total_points; i >= 0; i--) {
//         float x = x_start + i * dx;
//         float y = WAVE_AMPLITUDE_MM * sin(x);  // y = A * sin(x)
//         move_to(start_x_mm + (x / (2 * M_PI)) * WAVE_LENGTH_MM, y);
//     }

    
// }


